close all;
clear all;

V = load('~/Dropbox/Bonobo-ASLab-Share/steveshare/video-based/fusion-0.5.txt'); % blue_video_gaps
M = load('~/Dropbox/Bonobo-ASLab-Share/steveshare/video-based/baseline.txt'); % blue_mapper
M2 = load('~/Dropbox/Bonobo-ASLab-Share/steveshare/video-based/gaps-0.5.txt'); % blue_mapper_gaps

%scale_factor = 0.2;
%stop = 1384482877.0;
%stop = 1384482886.0;
%restart = 1384482880.0; 
%restart = 1384482887.0;

Vi(:,1) = M(:,1);
Vi(:,2) = M(:,2);
M2i(:,1) = M(:,1);
M2i(:,2) = M(:,2);


for iii = 1:6
	Vi(:,iii+2) = interp1(V(:,1), V(:,iii+2), M(:,1));
end

for iii = 1:7
	M2i(:,iii+2) = interp1(M2(:,1), M2(:,iii+2), M(:,1));
end

for iii = 7:size(V,2)-2
	Vi(:,iii+2) = interp1(V(:,1), V(:,iii+2), M(:,1));
end







errorx(:,1) = M(:,1);
errorx(:,2) = M(:,2);
errorx(:,3) = ((Vi(:,3)-M(:,3)).^2 + (Vi(:,4)-M(:,4)).^2 + (Vi(:,5)-M(:,5)).^2).^0.5;

iii = 1;
while (isnan(errorx(iii,3)))
	iii = iii + 1;
end
min_error = errorx(iii,3);
errorx(:,3) = abs(errorx(:,3) - min_error);

% to calculate quaternion angle

errorx(:,4) = min(abs(acos(2*( Vi(:,6).*M(:,6) + Vi(:,7).*M(:,7) + Vi(:,8).*M(:,8) + Vi(:,9).*M(:,9) ).^2 - 1)), pi/2-abs(acos(2*( Vi(:,6).*M(:,6) + Vi(:,7).*M(:,7) + Vi(:,8).*M(:,8) + Vi(:,9).*M(:,9) ).^2 - 1)));

% mapper with gaps
errorx(:,5) = ((M2i(:,3)-M(:,3)).^2 + (M2i(:,4)-M(:,4)).^2 + (M2i(:,5)-M(:,5)).^2).^0.5;
iii = 1;
while (isnan(errorx(iii,5)))
	iii = iii + 1;
end
min_error = errorx(iii,5);
errorx(:,5) = abs(errorx(:,5) - min_error);

sc = 1.0;
h = plot(1);
set (h(1), "linewidth", 16) 
%plot(errorx(:,1)-min(errorx(:,1)), errorx(:,4)*sc,'b');
%hold on;
plot(errorx(:,1)-min(errorx(:,1)), errorx(:,3),'b', 'LineWidth', 2);
hold on;
plot(errorx(:,1)-min(errorx(:,1)), errorx(:,5),'r-', 'LineWidth', 2);

for counter = 0:35
	%plot([(floor(errorx(1,1)) + counter -min(errorx(:,1))) (floor(errorx(1,1)) + counter -min(errorx(:,1)))], [0 1]);
	%counter = counter + 1;
end

%plot(Vi(:,1)-min(errorx(:,1)), scale_factor + scale_factor*Vi(:,10)/max(Vi(:,10)),'g');
%plot(Vi(:,1)-min(errorx(:,1)), scale_factor + scale_factor*Vi(:,11)/max(Vi(:,11)),'m');
%plot(Vi(:,1)-min(errorx(:,1)), scale_factor + scale_factor*Vi(:,12)/max(Vi(:,12)),'b');
%plot(Vi(:,1)-min(errorx(:,1)), scale_factor + scale_factor*Vi(:,13)/max(Vi(:,13)),'c');
%plot(Vi(:,1)-min(errorx(:,1)), scale_factor + scale_factor*Vi(:,15)/max(Vi(:,15)),'c');
%plot(M(:,1), M(:,3),'k');

if 1
	%plot([stop-min(errorx(:,1)) stop-min(errorx(:,1))], [0.0 2*scale_factor], 'k');
	%plot([restart-min(errorx(:,1)) restart-min(errorx(:,1))], [0.0 2*scale_factor], 'k');
	%plot([0 max(V(:,1))-min(errorx(:,1))], [scale_factor scale_factor], 'k');
	%axis([1384482875 1384482895 0.0 scale_factor]);
	axis([0 30.0 0.0 0.6]);
end

ylabel('translation error (m)', 'fontsize', 20);
xlabel('time (s)', 'fontsize', 20);
legend("proposed video-supported method", "conventional ICP-only method", "location", "northoutside")

%FL2= findall(L,'-property','FontSize');
%set(FL2,'FontSize',20);

%print '~/Dropbox/Bonobo-ASLab-Share/steveshare/video-based/evaluation_2.pdf'
